
#include "robot.h"

#include <iostream>
#include <string>
#include <functional>
#include <cmath>
#include <cassert>

// **THE TIME QUANT**
extern double dt;

const char* BONE_FILE_PREFIX="bone";

double normalise(double v)
{
  while(v > PI)
    v-=2*PI;
  while(v < -PI)
    v+=2*PI;
  return v;
}



/*
  ===conversion routines=============
*/

MAV_matrix ublas2mav(matr A)
{
  MAV_matrix M;
  for(int i=0; i<4; i++)
    for(int j=0; j<4; j++)
      M.mat[i][j]=A(i,j);
  return M;
}

void mav_set_from_ublas(MAV_matrix *M, matr A)
{
  for(int i=0; i<4; i++)
    for(int j=0; j<4; j++)
      M->mat[i][j]=A(i,j);
}

/*void mav_matrix_cpy(MAV_matrix *M,MAV_matrix s)
  {
  for(int i=0; i<4; i++)
  for(int j=0; j<4; j++)
  M->mat[i][j]=s.mat[i][j];
  }*/

/*
  ===model struct====================
*/
  
std::vector<double> substract(std::vector<double> a, std::vector<double> b)
{
  assert(a.size()==b.size());
  std::vector<double> r;
  for(int i=0; i<a.size(); i++){
    r.push_back(a[i]-b[i]);
  }
  return r;
}

std::vector<double> add(std::vector<double> a, std::vector<double> b)
{
  assert(a.size()==b.size());
  std::vector<double> r;
  for(int i=0; i<a.size(); i++){
    r.push_back(a[i]+b[i]);
  }
  return r;
}


std::vector<double> devide(std::vector<double> a, double v)
{
 
  std::vector<double> r;
  for(int i=0; i<a.size(); i++){
    r.push_back(a[i]/v);
  }
  return r;
}

  

bool model::load(matr Origin)
{
  origin=Origin;

  matrix_vector orients=forward_solution_T(0,0,0,0,0,0,origin);

  for(int i=0; i<BONES_COUNT; i++){
    std::ostringstream fname;
    fname<<BONE_FILE_PREFIX<<i<<".ac";
    if(!mav_compositeReadAC3D((char*)(fname.str().c_str()),
			      &bones[i],MAV_ID_MATRIX))
      {
	std::cerr<<"loading model "<<fname.str()<<" failed\n";
	return false;
      }
    mav_set_from_ublas(&(bones[i].matrix),orients[i]);
  }

  std::ostringstream fname_c;
  fname_c<<BONE_FILE_PREFIX<<BONES_COUNT<<"closed.ac";
  if(!mav_compositeReadAC3D((char*)(fname_c.str().c_str()),
			    &arm_closed,MAV_ID_MATRIX))
    {
      std::cerr<<"loading model "<<fname_c.str()<<" failed\n";
      return false;
    }

  std::ostringstream fname_o;
  fname_o<<BONE_FILE_PREFIX<<BONES_COUNT<<"opened.ac";
  if(!mav_compositeReadAC3D((char*)(fname_o.str().c_str()),
			    &arm_opened,MAV_ID_MATRIX))
    {
      std::cerr<<"loading model "<<fname_o.str()<<" failed\n";
      return false;
    }

   
  return true;
}

void model::orient(std::vector<double> Q)
{
    
  matrix_vector orients=forward_solution_T(Q,origin);

  for(int i=0; i<BONES_COUNT; i++)
    mav_set_from_ublas(&(bones[i].matrix),orients[i]);
  
  mav_set_from_ublas(&(arm_opened.matrix),orients[BONES_COUNT]);
  mav_set_from_ublas(&(arm_closed.matrix),orients[BONES_COUNT]);
}

void model::add_to_sms(MAV_SMS *Sms)
{
  sms=Sms;
  arm_closed_obj= mav_objectNew(mav_class_composite, &arm_closed);
  arm_opened_obj= mav_objectNew(mav_class_composite, &arm_opened);
  
  for(int i=0; i<BONES_COUNT; i++)
    mav_SMSObjectAdd(sms, mav_objectNew(mav_class_composite, &bones[i]));

  mav_SMSObjectAdd(sms,arm_closed_obj);
 
  
}

void model::set_closed()
{  
  mav_SMSObjectRmv(sms,arm_opened_obj); 
  mav_SMSObjectRmv(sms,arm_closed_obj);
  mav_SMSObjectAdd(sms,arm_closed_obj);
}

void model::set_opened()
{
  mav_SMSObjectRmv(sms,arm_opened_obj); 
  mav_SMSObjectRmv(sms,arm_closed_obj);
  mav_SMSObjectAdd(sms,arm_opened_obj);
}

		       

/*
  ===Robot class====================
*/
  
Robot::Robot(std::vector<double> Q)
{
   path_points.push_back(Q);
   speed=50;
   k1=Plus; // JOINT 2 LEFT
   k2=Minus; //JOINT 3 UP
   W=Plus;
}

void Robot::set_speed(double val)
{
  speed=val;
  //clear previously calculated trace
  if(path_points.size()>1){
    std::vector<double> tmp=path_points[0];
    path_points.clear();
    path_points.push_back(tmp);
  }

}

void Robot::move(double x, double y, double z, double o, double a, double t)
{
 std::cout<<"passed to move: o="<<o<<" a="<<a<<" t="<<t<<std::endl;
  matr dest=forward_solution_T(path_points[0]).at(5);
  ublas::vector<double> rot=R2oat(dest);

  dest=build_trans(dest(0,3)+x,
		   dest(1,3)+y,
		   dest(2,3)+z,
		   rot(0)+o,
		   rot(1)+a,
		   rot(2)+t);
  
   gos(dest);
}

void Robot::move(std::vector<double> P)
{
  move(P[0],P[1],P[2],P[3],P[4],P[5]);
}

void Robot::gosnear(std::vector<double> P,double shift)
{
  clear_path();
  matr pos=build_trans(P);
  
  for(int i=0; i<3; i++){
    P[i]-=pos(i,2)*shift;
    std::cerr<<"shift"<<i<<" "<<-pos(i,2)*shift<<std::endl;
  }
  
  gos(P);
}

    
  

void Robot::gos(std::vector<double> P)
{
  gos(build_trans(P[0],P[1],P[2],P[3],P[4],P[5]));
}

void Robot::gos(matr dest)
{
  //removing remains of previous trace
  clear_path();
  
  matr pos=forward_solution_T(path_points[0]).at(5);
  matrix_vector quants;
  try{
    quants=quantise_translate(pos,dest,speed*dt);
    for(long i=0; i<quants.size(); i++){
      std::vector<double> Q=reverse_solution(quants[i],k1,k2,W);
    for(int j=0; j<Q.size(); j++)
      Q[j]=normalise(Q[j]);
      
    path_points.push_back(Q);
    }
    
  }
  catch(std::string err_msg){
    std::cerr<<"Error while trace building : "<<err_msg<<std::endl;

    clear_path();
    return;
  }
  
}

std::vector<double> Robot::xyz()
{
  matr pos=forward_solution_T(path_points[0]).at(5);
  return T2xyz(pos);
}


void Robot::clear_path()
{
   if(path_points.size()>1){
    std::vector<double> tmp=path_points[0];
    path_points.clear();
    path_points.push_back(tmp);
   }
}



void Robot::go(std::vector<double> Q)
{
  clear_path();
  std::vector<double> dist=substract(Q,path_points[0]);
  for(int i=0; i<dist.size(); i++)
    dist[i]=normalise(dist[i]);
      
  double maxdist=0;
  for(int i=0; i<dist.size(); i++)
    if(maxdist<std::fabs(dist[i]))
      maxdist=std::fabs(dist[i]);
  long count=(long)(maxdist/(speed*dt/200));
  std::vector<double> delta=devide(dist,count);
  std::cerr<<"COUNT "<<count<<std::endl;
  for(long i=0; i<count; i++){
    std::vector<double> next=add(path_points[i],delta);
    for(int i=0; i<next.size(); i++)
      next[i]=normalise(next[i]);
    path_points.push_back(next);
  }
  
  
}

void Robot::close()
{
  arm.set_closed();
}

void Robot::open()
{
  arm.set_opened();
}

  

bool Robot::traverse()
  {
        
    if(path_points.size()==1){
      arm.orient(path_points[0]);
      return false;
    }
    if(path_points.size()>1){
      arm.orient(path_points[1]);
      path_points.pop_front();
      return true;
      
    }
  }
